{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 移动操作基本运动控制\n",
    "\n",
    "## 硬件准备\n",
    "\n",
    "材料：\n",
    "\n",
    "小车：移动底座 M*1、I模块*4、四麦克朗姆轮 *4 \n",
    "\n",
    "操作臂（5自由度）：I模块*2、T模块*3、G模块*1\n",
    "\n",
    "\n",
    "\n",
    "## 硬件连接\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "## 关节配置\n",
    "\n",
    "将硬件连接的关节ID次序更新到配置文件中\n",
    "\n",
    "\n",
    "\n",
    "配置完后启动设备\n",
    "\n",
    "\n",
    "# 代码说明\n",
    "\n",
    "### 导入库"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "#!/usr/bin/python\n",
    "# -*- coding: UTF-8 -*-\n",
    "import sys;\n",
    "import os\n",
    "sys.path.append(\"..\")\n",
    "sys.path.append(\"../../\")\n",
    "from lib.BwRobotLib import BwRobotLib\n",
    "from RobotCar.CarRobot import CarRobot\n",
    "from RobotManipulator.ManipulatorRobot import ManipulatorRobot\n",
    "import keyboard\n",
    "import time"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 声明库对象"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "#声明库及机器人对象\n",
    "robotlib = BwRobotLib()\n",
    "carRobot = CarRobot(robotlib)\n",
    "manipulator = ManipulatorRobot(robotlib)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 连接设备"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "连接成功\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "1"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#连接机器人设备\n",
    "robotlib.connectRobot(\"192.168.1.107\",55001)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 配置小车并初始化"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "请选择轮子模式 :\n",
      "2 : 两轮\n",
      "3 : 三轮\n",
      "4 : 四向轮\n",
      "5 : 四麦克朗姆轮\n",
      "6 : 四并橡胶轮\n"
     ]
    },
    {
     "name": "stdin",
     "output_type": "stream",
     "text": [
      " 5\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Total Ids Count=10\n",
      "检测到的模块数： 10\n",
      "Id： 20  Position:8\n",
      "Id： 21  Position:11\n",
      "Id： 22  Position:4\n",
      "Id： 23  Position:6\n",
      "Id： 24  Position:9\n",
      "Id： 30  Position:9\n",
      "Id： 31  Position:9\n",
      "Id： 25  Position:9\n",
      "Id： 32  Position:9\n",
      "Id： 40  Position:9\n",
      "任意键回车继续....... (输入 0 回车重新扫描)\n"
     ]
    },
    {
     "name": "stdin",
     "output_type": "stream",
     "text": [
      " \n"
     ]
    }
   ],
   "source": [
    "#配置轮式模式\n",
    "print(\"请选择轮子模式 :\")\n",
    "print(\"2 : 两轮\")\n",
    "print(\"3 : 三轮\")\n",
    "print(\"4 : 四向轮\")\n",
    "print(\"5 : 四麦克朗姆轮\")\n",
    "print(\"6 : 四并橡胶轮\")\n",
    "mode = robotlib.inputNum()\n",
    "\n",
    "#加载配置\n",
    "robotlib.module_ids=robotlib.scanNConfigCarIds()\n",
    "\n",
    "#配置机器人\n",
    "#配置ID:ids F(4),B(11),l1(8),l2(8),l3(10),l4(11),r1(4),r2(7),r3(6),r4(6) 【TOP:9】\n",
    "carRobot.bindIds(robotlib.module_ids,mode)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 配置操作臂并初始化"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "读取ID配置文件:\n",
      "#操作臂\n",
      "\n",
      "UP:24,30,31,25,32,40\n",
      "关节模块24位置：-156\n",
      "关节模块30位置：-90\n",
      "关节模块31位置：-90\n",
      "关节模块25位置：118\n",
      "关节模块32位置：-2\n",
      "关节模块40位置：5\n"
     ]
    }
   ],
   "source": [
    "#加载配置（兼容VS项目启动方式及文件直接启动方式）\n",
    "import os,inspect\n",
    "fileName = inspect.getframeinfo(inspect.currentframe()).filename\n",
    "folderPath     = os.path.dirname(os.path.abspath(fileName))\n",
    "cfile=folderPath+\"/config.txt\"\n",
    "robotlib.loadConfig(cfile)\n",
    "\n",
    "\n",
    "#配置操作臂\n",
    "manipulator.bindIds(robotlib.module_ids)\n",
    "\n",
    "#读取当前坐标\n",
    "manipulator.readAngles()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 小车控制指令处理"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "#处理小车控制指令\n",
    "def handleCarControlKeys(key):\n",
    "    isDown = key.event_type == 'down'\n",
    "    if not isDown:\n",
    "        carRobot.stop()\n",
    "        return\n",
    "\n",
    "    cmd = key.name\n",
    "    if not(cmd == '8' or cmd == '3'):\n",
    "        if(carRobot.is_running):\n",
    "            return\n",
    "\n",
    "    if(cmd == '1'):\n",
    "        carRobot.goForward()\n",
    "    if(cmd == '2'):\n",
    "        carRobot.goBack()\n",
    "    if(cmd == '3'):\n",
    "        carRobot.stop_wheel()\n",
    "    if(cmd == '4'):\n",
    "        carRobot.turnLeft()\n",
    "    if(cmd == '5'):\n",
    "        carRobot.turnRight()\n",
    "    if(cmd == '6'):\n",
    "        carRobot.runLeft()\n",
    "    if(cmd == '7'):\n",
    "        carRobot.runRight()\n",
    "    pass"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 操作臂控制指令"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "#控制空间坐标\n",
    "def controlCoordinate(key):\n",
    "    cmd = key.name\n",
    "    #print(cmd)\n",
    "    #X\n",
    "    if(cmd == 'i'):\n",
    "        val = manipulator.spatial_coordinate[0] + 1\n",
    "        manipulator.spatial_coordinate[0] = val\n",
    "    if(cmd == 'k'):\n",
    "        val = manipulator.spatial_coordinate[0] - 1\n",
    "        manipulator.spatial_coordinate[0] = val\n",
    "\n",
    "    #Y\n",
    "    if(cmd == 'o'):\n",
    "        val = manipulator.spatial_coordinate[1] + 1\n",
    "        manipulator.spatial_coordinate[1] = val\n",
    "    if(cmd == 'l'):\n",
    "        val = manipulator.spatial_coordinate[1] - 1\n",
    "        manipulator.spatial_coordinate[1] = val\n",
    "\n",
    "    #Z\n",
    "    if(cmd == 'p'):\n",
    "        val = manipulator.spatial_coordinate[2] + 1\n",
    "        manipulator.spatial_coordinate[2] = val\n",
    "    if(cmd == 'm'):\n",
    "        val = manipulator.spatial_coordinate[2] - 1\n",
    "        manipulator.spatial_coordinate[2] = val\n",
    "\n",
    "    #Roll\n",
    "    if(cmd == 'z'):\n",
    "        val = manipulator.spatial_coordinate[3] + 1\n",
    "        manipulator.spatial_coordinate[3] = val\n",
    "    if(cmd == 'x'):\n",
    "        val = manipulator.spatial_coordinate[3] - 1\n",
    "        manipulator.spatial_coordinate[3] = val\n",
    "\n",
    "    #Pitch\n",
    "    if(cmd == 'c'):\n",
    "        val = manipulator.spatial_coordinate[4] + 1\n",
    "        manipulator.spatial_coordinate[4] = val\n",
    "    if(cmd == 'v'):\n",
    "        val = manipulator.spatial_coordinate[4] - 1\n",
    "        manipulator.spatial_coordinate[4] = val\n",
    "\n",
    "     #Yaw\n",
    "    if(cmd == 'b'):\n",
    "        val = manipulator.spatial_coordinate[5] + 1\n",
    "        manipulator.spatial_coordinate[5] = val\n",
    "    if(cmd == 'n'):\n",
    "        val = manipulator.spatial_coordinate[5] - 1\n",
    "        manipulator.spatial_coordinate[5] = val\n",
    "\n",
    "    manipulator.pushSpatialCoordinate()\n",
    "    pass\n",
    "\n",
    "#控制关节角度\n",
    "def controlAngles(key):\n",
    "    cmd = key.name\n",
    "    #print(cmd)\n",
    "    #I\n",
    "    if(cmd == 'q'):\n",
    "        val = manipulator.position_angles[0] + 1\n",
    "        manipulator.position_angles[0] = robotlib.fixValue(val,180,-180)\n",
    "    if(cmd == 'a'):\n",
    "        val = manipulator.position_angles[0] - 1\n",
    "        manipulator.position_angles[0] = robotlib.fixValue(val,180,-180)\n",
    "\n",
    "    #T\n",
    "    if(cmd == 'w'):\n",
    "        val = manipulator.position_angles[1] + 1\n",
    "        manipulator.position_angles[1] = robotlib.fixValue(val,90,-90)\n",
    "    if(cmd == 's'):\n",
    "        val = manipulator.position_angles[1] - 1\n",
    "        manipulator.position_angles[1] = robotlib.fixValue(val,90,-90)\n",
    "\n",
    "    #T\n",
    "    if(cmd == 'e'):\n",
    "        val = manipulator.position_angles[2] + 1\n",
    "        manipulator.position_angles[2] = robotlib.fixValue(val,90,-90)\n",
    "    if(cmd == 'd'):\n",
    "        val = manipulator.position_angles[2] - 1\n",
    "        manipulator.position_angles[2] = robotlib.fixValue(val,90,-90)\n",
    "\n",
    "    #I\n",
    "    if(cmd == 'r'):\n",
    "        val = manipulator.position_angles[3] + 1\n",
    "        manipulator.position_angles[3] = robotlib.fixValue(val,180,-180)\n",
    "    if(cmd == 'f'):\n",
    "        val = manipulator.position_angles[3] - 1\n",
    "        manipulator.position_angles[3] = robotlib.fixValue(val,180,-180)\n",
    "\n",
    "    #T\n",
    "    if(cmd == 't'):\n",
    "        val = manipulator.position_angles[4] + 1\n",
    "        manipulator.position_angles[4] = robotlib.fixValue(val,90,-90)\n",
    "    if(cmd == 'g'):\n",
    "        val = manipulator.position_angles[4] - 1\n",
    "        manipulator.position_angles[4] = robotlib.fixValue(val,90,-90)\n",
    "\n",
    "    max = 180\n",
    "    min = -180\n",
    "    if(manipulator.ids_size <= 6):\n",
    "        max = 90\n",
    "        min = 0\n",
    "     #G/I\n",
    "    if(cmd == 'y'):      \n",
    "        val = manipulator.position_angles[5] + 1\n",
    "        manipulator.position_angles[5] = robotlib.fixValue(val,max,min)\n",
    "    if(cmd == 'h'):\n",
    "        val = manipulator.position_angles[5] - 1\n",
    "        manipulator.position_angles[5] = robotlib.fixValue(val,max,min)\n",
    "\n",
    "    #G\n",
    "    if(cmd == 'u'):\n",
    "        if(manipulator.ids_size > 6):        \n",
    "           val = manipulator.position_angles[6] + 1\n",
    "           manipulator.position_angles[6] = robotlib.fixValue(val,90,0)\n",
    "    if(cmd == 'j'):\n",
    "        if(manipulator.ids_size > 6):   \n",
    "           val = manipulator.position_angles[6] - 1\n",
    "           manipulator.position_angles[6] = robotlib.fixValue(val,90,0)\n",
    "\n",
    "    manipulator.pushJointsAngles()\n",
    "    pass"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "### 键盘控制命令处理"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "#处理控制键\n",
    "def handleControlKeys(key):\n",
    "     cmd = key.name\n",
    "     #退出监测\n",
    "     if(cmd == '0'):\n",
    "        #keyboard.unhook_all()\n",
    "        return\n",
    "\n",
    "     if(cmd == 'i' or cmd == 'k' or cmd == 'o' or cmd == 'l' or cmd == 'p' or cmd == 'm' or cmd == 'z' or cmd == 'x' or cmd == 'c' or cmd == 'v' or cmd == 'b' or cmd == 'n'):\n",
    "        controlCoordinate(key) \n",
    "\n",
    "     if(cmd == 'q' or cmd == 'a' or cmd == 'w' or cmd == 's' or cmd == 'e' or cmd == 'd' or cmd == 'r' or cmd == 'f' or cmd == 't' or cmd == 'g' or cmd == 'y' or cmd == 'h' or cmd == 'u' or cmd == 'j'):\n",
    "        controlAngles(key) \n",
    "\n",
    "     if(cmd == '1' or cmd == '2' or cmd == '3' or cmd == '4' or cmd == '5' or cmd == '6' or cmd == '7'):\n",
    "        handleCarControlKeys(key) "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 控制提示\n",
    "\n",
    "为防止与JupterLab的快捷键冲突，请最小化当前进程或在其它程序窗口执行以下命令"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "#控制指示\n",
    "def controlTips():\n",
    "    print(\"键盘控制角度: 增加 / 减少\")\n",
    "    print(\"关节 1 : Q/A;\")\n",
    "    print(\"关节 2 : W/S;\")\n",
    "    print(\"关节 3 : E/D;\")\n",
    "    print(\"关节 4 : R/F;\")\n",
    "    print(\"关节 5 : T/G;\")\n",
    "    print(\"关节 6 : Y/H;\")\n",
    "    print(\"关节 7 : U/J;\")\n",
    "\n",
    "    print(\"空间坐标 X : I/K;\")\n",
    "    print(\"空间坐标 Y : O/L;\")\n",
    "    print(\"空间坐标 Z : P/M;\")\n",
    "    print(\"空间坐标 Roll: Z/X;\")\n",
    "    print(\"空间坐标 Pitch: C/V;\")\n",
    "    print(\"空间坐标 Yaw: B/N\")\n",
    "\n",
    "    print(\"小车移动 ******************\")\n",
    "    print(\"1 : 向前走\")\n",
    "    print(\"2 : 向后走\")\n",
    "    print(\"3 : 停止\")\n",
    "    print(\"4 : 左转\")\n",
    "    print(\"5 : 右转\")\n",
    "    print(\"6 : 左走（5 : 四麦克朗姆轮）\")\n",
    "    print(\"7 : 右走（5 : 四麦克朗姆轮）\")\n",
    "\n",
    "    print(\"-----------------------\")\n",
    "    print(\"0 :退出\")\n",
    "    keyboard.hook(handleControlKeys)\n",
    "    #参考Python 键盘/鼠标监听及控制：https://www.jianshu.com/p/8e508c6a05ce\n",
    "    keyboard.wait('0')\n",
    "    keyboard.unhook_all()\n",
    "    pass"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 主入口"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "键盘控制角度: 增加 / 减少\n",
      "关节 1 : Q/A;\n",
      "关节 2 : W/S;\n",
      "关节 3 : E/D;\n",
      "关节 4 : R/F;\n",
      "关节 5 : T/G;\n",
      "关节 6 : Y/H;\n",
      "关节 7 : U/J;\n",
      "空间坐标 X : I/K;\n",
      "空间坐标 Y : O/L;\n",
      "空间坐标 Z : P/M;\n",
      "空间坐标 Roll: Z/X;\n",
      "空间坐标 Pitch: C/V;\n",
      "空间坐标 Yaw: B/N\n",
      "小车移动 ******************\n",
      "1 : 向前走\n",
      "2 : 向后走\n",
      "3 : 停止\n",
      "4 : 左转\n",
      "5 : 右转\n",
      "6 : 左走（5 : 四麦克朗姆轮）\n",
      "7 : 右走（5 : 四麦克朗姆轮）\n",
      "-----------------------\n",
      "0 :退出\n",
      "四麦克朗姆轮向前走...\n",
      "停止\n",
      "四麦克朗姆轮向后走...\n",
      "停止\n",
      "停止\n",
      "停止\n",
      "停止\n",
      "停止\n",
      "停止\n",
      "停止\n",
      "停止\n",
      "停止\n",
      "停止\n",
      "四麦克朗姆轮左转...\n",
      "停止\n"
     ]
    }
   ],
   "source": [
    "controlTips()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
